# Vision Interface for soul planner
# By: Camiel Verschoor

import vision_IR as vision
import threading
#import houghCircles_IR as ball
import longDistanceTracker_IR as ball
from naoqi import ALProxy

leds = ALProxy("ALLeds", "localhost",9559) 

class goalFilter(threading.Thread):
        run = True
        active = False
        color = None
        image = None
        head = None
        info = None

        def __init__(self, threadID, name, color):
                threading.Thread.__init__(self)
                self.threadID = threadID
                self.name = name
                self.color = color

        def run(self):
                while self.run:
                        while self.active:
                                # Check for No input
                                if(self.image == None or self.head[0] == None or self.head[1] == None):
                                        self.active = False
                                        self.image = None
                                        self.head = None
                                else:
                                        self.info = vision.findGoal(self.image, self.color, self.head)
                                        self.active = False

        # Lets the thread end
        def off(self):
                self.run = False

        # Puts the filter on
        def on(self):
                self.active = True

        # Checks if the filter is running
        def isActive(self):
                return self.active

        # Gets the info
        def get(self):
                x = self.info
                self.image = None
                if x == None:
                        return None
                else:
                        return x

        # Puts image
        def put(self, image, head):
                self.image = image
                self.head = head

blueGoal = goalFilter(98, "yGoalFilter", "blue")
yellowGoal = goalFilter(99, "bGoalFilter", "yellow")

blueGoal.start()
yellowGoal.start()

# Vision ID
visionID = vision.subscribe()
# METHODS FOR VISION

def findGoal():
        # Take snapshot and measure head angles
        (image, head) = vision.snapShot(visionID)
        # Filter the snapshots and return the angle and the color
        angle = filterGoal(image, head)
        if angle == None:
                leds.fadeRGB("LeftFaceLeds",0x00ff0000, 0) #no goal, led turns red
        else: 
                leds.fadeRGB("LeftFaceLeds", 0x0000ff00, 0)
        return angle

def findBall():
        # Take snapshot and measure head angles
        (image, head) = vision.snapShot(visionID)
        # Filter the snapshots and return the angle and the color
        ballLoc = ball.run(image, head)
        if ballLoc == None:
                leds.fadeRGB("RightFaceLeds",0x00ff0000, 0) #no ball, led turns red
        else: 
                leds.fadeRGB("RightFaceLeds", 0x0000ff00, 0)
        return ballLoc

# FUNCTION FOR GOAL
def filterGoal(image, head):
        # Transfer the info to the threads
        blueGoal.put(image, head)
        yellowGoal.put(image, head)
        # Run the threads
        blueGoal.on()
        yellowGoal.on()
        # Join threads
        while blueGoal.isActive() and yellowGoal.isActive():
                pass
        # Retrieve information
        blue = blueGoal.get()
        yellow = yellowGoal.get()
        # Check
        if not(blue == None):
                angle = ("Blue", blue)
        elif not(yellow == None):
                angle = ("Yellow", yellow)
        else:
                angle = None
        return angle
        
# Turn the threads off
def off():
        blueGoal.off()
        yellowGoal.off()
